﻿using AgCIM.Tools.IO.Shape;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static System.Windows.Forms.AxHost;

namespace CarSimulate
{
    internal class CarAgent
    {
        public CarInLineStruct CarPos=new CarInLineStruct();
        
        public PointF NextTrafficNode { get; set; }
        private TrafficLine m_currentLine;
        private List<TrafficLine> m_SibleLines = new List<TrafficLine>();
        
        // 添加运动轨迹存储
        private List<Shapefile_Point> m_movementTrajectory = new List<Shapefile_Point>();
        public List<Shapefile_Point> MovementTrajectory { get { return m_movementTrajectory; } }
        
        public TrafficLine CurrentLine
        {
            get { return m_currentLine; }
            set
            {
                if(m_currentLine== value)
                {
                    return;
                }
                m_currentLine = value;
                m_SibleLines.Clear();
                for (var i=0;i< CurrentRoadManager.AllLines.Count;i++)
                {
                    if (CurrentRoadManager.AllLines[i].LinePId== m_currentLine.LinePId)
                    {
                        m_SibleLines.Add(CurrentRoadManager.AllLines[i]);
                    }
                }
                if (m_SibleLines.Count > 1)
                {

                }
            }
        }
        private TrafficLine m_targetLine;
        public static Random RandomItem { get; set; }
        public RoadManager CurrentRoadManager { get; set; }

        public string State { get; private set; }
        public float Speed { get; internal set; }
        
        public int Id { get; internal set; }

        private TrafficLine TargetLineId
        {
            get
            {
                if (m_targetLine == null) return CurrentLine; 
                return m_targetLine;
            }
        }

        private int m_keepCount = 0;
        private int m_wanneSwithCount = 0;
        private StringBuilder m_records=new StringBuilder();

        public void Start()
        {
            m_records = new StringBuilder();
            // 清空运动轨迹
            m_movementTrajectory.Clear();
        }
        
        // 添加更新轨迹方法
        public void UpdateTrajectory(Shapefile_Point currentPoint)
        {
            // 如果轨迹为空，直接添加第一个点
            if (m_movementTrajectory.Count == 0)
            {
                m_movementTrajectory.Add(currentPoint);
                return;
            }
            
            // 如果只有一个点，添加第二个点
            if (m_movementTrajectory.Count == 1)
            {
                m_movementTrajectory.Add(currentPoint);
                return;
            }
            
            // 计算当前方向和上一个方向
            var lastPoint = m_movementTrajectory[m_movementTrajectory.Count - 1];
            var secondLastPoint = m_movementTrajectory[m_movementTrajectory.Count - 2];
            
            // 计算上一段的方向向量
            var prevDirection = new Shapefile_Point(lastPoint.X - secondLastPoint.X, lastPoint.Y - secondLastPoint.Y);
            // 计算当前段的方向向量
            var currentDirection = new Shapefile_Point(currentPoint.X - lastPoint.X, currentPoint.Y - lastPoint.Y);
            
            // 计算方向角度差异（使用点积判断方向变化）
            var prevLength = Math.Sqrt(prevDirection.X * prevDirection.X + prevDirection.Y * prevDirection.Y);
            var currentLength = Math.Sqrt(currentDirection.X * currentDirection.X + currentDirection.Y * currentDirection.Y);
            
            if (prevLength > 0 && currentLength > 0)
            {
                // 归一化方向向量
                var prevNormalized = new Shapefile_Point((float)(prevDirection.X / prevLength), (float)(prevDirection.Y / prevLength));
                var currentNormalized = new Shapefile_Point((float)(currentDirection.X / currentLength), (float)(currentDirection.Y / currentLength));
                
                // 计算点积（cos值）
                var dotProduct = prevNormalized.X * currentNormalized.X + prevNormalized.Y * currentNormalized.Y;
                
                // 如果方向变化很小（cos值接近1），则删除上一个点
                if (dotProduct > 0.95) // 约18度以内的变化认为是直线
                {
                    m_movementTrajectory.RemoveAt(m_movementTrajectory.Count - 1);
                }
            }
            
            // 添加当前点
            m_movementTrajectory.Add(currentPoint);
        }
        internal void MoveNext(List<CarAgent> otherCars)
        {
            CarPos.Save();
            State = null;
            if (m_targetLine == null)
            {
                var switchLine = CurrentLine;
                if (m_wanneSwithCount > 5&& m_SibleLines.Count>1)
                {//找到其中的变道
                    switchLine = SelectPallelRoad();
                    if(switchLine!= null&&CanGoNextChain(switchLine, otherCars) == false)
                    {
                        switchLine = CurrentLine;
                        m_wanneSwithCount -= 10;
                        if (m_wanneSwithCount < 0) m_wanneSwithCount = 0;
                    }
                    if (switchLine == null)
                    {
                        switchLine = CurrentLine;
                    }
                }
                if (switchLine == CurrentLine)
                {
                    //车道保持
                    State = "K";
                    m_keepCount++;

                    CarPos.AbsInLine = true;
                    CurrentLine.GetNextPoint(CarPos, Speed);

                    //还有没有走完的路
                    if (CarPos.LeftDistanceToGo > 0 && CurrentLine.NextLines.Count != 0)
                    {
                        CurrentLine = CurrentLine.NextLines[RandomItem.Next(0, CurrentLine.NextLines.Count)];
                        CurrentLine.GetNextPoint(CarPos, CarPos.LeftDistanceToGo);
                    }

                    return;
                }
                m_targetLine = switchLine;
                m_keepCount = 0;
                m_wanneSwithCount = 0;
            }
            //正在变道
            var cpClone = new PointF(CarPos.CurrentPoint.X, CarPos.CurrentPoint.Y);
            //意向的目标位置
            CarPos.CurrentPoint = m_targetLine.GetProjectionPoint(CarPos.CurrentPoint);
            //超速通过
            m_targetLine.GetNextPoint(CarPos, Speed*1.5);
            var vp = MoveTowards(cpClone, CarPos.CurrentPoint, Speed);
            var dis = CarDistance.DistanceToLineSegmentOptimized(vp, m_targetLine.StartPoint, m_targetLine.EndPoint);
            State = "S" + m_targetLine.LinePId + " - " + dis.ToString("f0");
            m_wanneSwithCount = 0;
            if (dis <= 3)
            {
                CarPos.AbsInLine = true;
                CurrentLine = m_targetLine;
                m_targetLine = null;
            }
            else
            {
                CarPos.CurrentPoint = vp;
                CarPos.AbsInLine = false;
            }
        }
        /// <summary>
        /// 是否可以到另外一个车道
        /// </summary>
        /// <param name="switchLine"></param>
        /// <param name="otherCars"></param>
        /// <returns></returns>
        private bool CanGoNextChain(TrafficLine switchLine, List<CarAgent> otherCars)
        {
            for (var i = 0; i < otherCars.Count; i++)
            {
                if (otherCars[i] == this) continue;
                if (otherCars[i].TargetLineId == switchLine)
                {
                    double dx = CarPos.CurrentPoint.X - otherCars[i].CarPos.CurrentPoint.X;
                    double dy = CarPos.CurrentPoint.Y - otherCars[i].CarPos.CurrentPoint.Y;
                    var dis = Math.Sqrt(dx * dx + dy * dy);
                    if (dis < 40)
                    {
                        return false;
                    }
                }
            }
            return true;
        }


        /// <summary>
        /// 从起点向终点方向移动指定距离
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="end">终点</param>
        /// <param name="distance">移动距离</param>
        /// <returns>移动后的新点坐标</returns>
        public PointF MoveTowards(PointF start, PointF end, double distance = 10.0)
        {
            // 计算方向向量
            double dx = end.X - start.X;
            double dy = end.Y - start.Y;

            // 计算两点间距离
            double length = Math.Sqrt(dx * dx + dy * dy);

            // 如果两点重合或距离为0，直接返回终点
            if (length <= 0)
                return end;

            // 计算移动比例
            double ratio = distance / length;

            // 如果移动距离超过剩余距离，返回终点
            if (ratio >= 1.0)
                return end;

            // 计算新坐标
            double newX = start.X + dx * ratio;
            double newY = start.Y + dy * ratio;

            return new PointF((float)newX, (float)newY);
        }

        #region 选择其中一条并行性变道
        private TrafficLine SelectPallelRoad()
        {
            for (var i = 0; i < m_SibleLines.Count; i++)
            {
                if (m_SibleLines[i] == CurrentLine) { continue; }
                var projectDis = CarDistance.DistanceToLineSegmentOptimized(CarPos.CurrentPoint, m_SibleLines[i].StartPoint, m_SibleLines[i].EndPoint);
                if (projectDis < 30)
                {
                    return m_SibleLines[i];
                }
                else
                {

                }
            }
            return null;
        }
        #endregion

        #region 是否可以超车
        internal bool WannaColl(List<CarAgent> agents)
        {
            var lineId1 = m_targetLine != null ? m_targetLine : this.CurrentLine;
            int selNearCarId = -1;
            double selNearCarRand = double.MaxValue;
            for (var i = 0; i < agents.Count; i++)
            {
                if (agents[i] == this) continue;
                var lineId2 = agents[i].m_targetLine != null ? agents[i].m_targetLine : agents[i].CurrentLine;
                if (lineId1 != lineId2)
                {
                    continue;
                }
                if (this.CarPos.CurrentLineRank > agents[i].CarPos.CurrentLineRank)
                {
                    continue;
                }
                if(selNearCarRand> agents[i].CarPos.CurrentLineRank)
                {
                    selNearCarRand = agents[i].CarPos.CurrentLineRank;
                    selNearCarId = i;
                }
            }
            if (selNearCarId==-1)
            {
                return false;
            }
            var dis = Math.Abs(lineId1.Length * (this.CarPos.CurrentLineRank - agents[selNearCarId].CarPos.CurrentLineRank));
            if (dis > 15) return false;
            if (dis <= 6)
            {
                CarPos.Slow(0);

                m_wanneSwithCount++;
            }
            else if (dis < 8)
            {
                CarPos.Slow(0.2);

                m_wanneSwithCount++;
            }
            else if (dis < 12)
            {
                CarPos.Slow(0.4);
                m_wanneSwithCount++;
            }
            else if (dis < 15)
            {
                CarPos.Slow(0.8);
                m_wanneSwithCount++;
            }
            return true;
        }


        #endregion

        #region 初始查找所在路线
        internal void FindRoad(RoadManager roadManager)
        {
            var minPos=double.MaxValue;
            var selRoad = 0;
            for(var i = 0; i < roadManager.AllLines.Count; i++)
            {
                var dis = CarDistance.DistanceToLineSegmentOptimized(CarPos.CurrentPoint,
                    roadManager.AllLines[i].StartPoint,
                    roadManager.AllLines[i].EndPoint);
                if(minPos> dis)
                {
                    minPos = dis;
                    selRoad = i;
                }
            }
            this.CurrentRoadManager=roadManager;
            CurrentLine = roadManager.AllLines[selRoad];
        }
        #endregion
    }
}
